#include "physics.h"

Physics::Physics(btVector3 &gravity)
{
	// Broadphase
	mBroadphase = new btDbvtBroadphase();

	// Collision configuration
	mCollisionConfiguration = new btDefaultCollisionConfiguration();

	// Collision dispatcher
	mDispatcher = new btCollisionDispatcher(mCollisionConfiguration);

	// Constraint solver
	mSolver = new btSequentialImpulseConstraintSolver();

	// Dynamics world
	mDynamicsWorld = new btDiscreteDynamicsWorld(mDispatcher, mBroadphase, mSolver, mCollisionConfiguration);
	mDynamicsWorld->setGravity(gravity);
}
Physics::~Physics()
{
	delete mDynamicsWorld;
	delete mSolver;
	delete mDispatcher;
	delete mCollisionConfiguration;
	delete mBroadphase;
}
void Physics::stepSimulation(float deltaTime)
{
	if(mDynamicsWorld)
		mDynamicsWorld->stepSimulation(deltaTime, MAX_SUBSTEPS);
}
void Physics::updateVehicle(float deltaTime)
{
}
btRigidBody* Physics::addBox(btVector3 &halfExtents, btScalar mass, btVector3 &position, btQuaternion &orientation)
{
	btVector3 inertia;
	btTransform transform;

	transform.setIdentity();
	transform.setOrigin(position);
	transform.setRotation(orientation);

	btCollisionShape *shape = new btBoxShape(halfExtents);
	shape->calculateLocalInertia(mass, inertia);

	btDefaultMotionState *motionState = new btDefaultMotionState(transform);
	btRigidBody *body = new btRigidBody(mass, motionState, shape, inertia);
	body->setFriction(DEFAULT_FRICTION);
	body->setDamping(DEFAULT_LINEAR_DAMPING, DEFAULT_ANGULAR_DAMPING);

	mDynamicsWorld->addRigidBody(body);

	if(mass > 0.0)
		mDynamicBodyList.push_back(body);
	else
		mStaticBodyList.push_back(body);

	return body;
}
btRigidBody* Physics::addSphere(btScalar radius, btScalar mass, btVector3 &position, btQuaternion &orientation)
{
	btVector3 inertia;
	btTransform transform;

	transform.setIdentity();
	transform.setOrigin(position);
	transform.setRotation(orientation);

	btCollisionShape *shape = new btSphereShape(radius);
	shape->calculateLocalInertia(mass, inertia);

	btDefaultMotionState *motionState = new btDefaultMotionState(transform);
	btRigidBody *body = new btRigidBody(mass, motionState, shape, inertia);
	body->setFriction(DEFAULT_FRICTION);
	body->setDamping(DEFAULT_LINEAR_DAMPING, DEFAULT_ANGULAR_DAMPING);

	mDynamicsWorld->addRigidBody(body);

	if(mass > 0.0)
		mDynamicBodyList.push_back(body);
	else
		mStaticBodyList.push_back(body);

	return body;
}
void Physics::setupVehicle(btVector3 &halfExtents, btScalar mass, btVector3 &position, btQuaternion &orientation)
{
	btVector3 inertia;
	btTransform transform;

	airplaneMass = mass;

	transform.setIdentity();
	transform.setOrigin(position);
	transform.setRotation(orientation);


	btCollisionShape *shape = new btBoxShape(halfExtents);
	shape->calculateLocalInertia(mass, inertia);

	btDefaultMotionState *motionState = new btDefaultMotionState(transform);
	mVehicleBody = new btRigidBody(mass, motionState, shape, inertia);
	mVehicleBody->setActivationState(DISABLE_DEACTIVATION);
	mVehicleBody->setDamping(0.2,0.5);

	
	//moveVehicle(0,0,0);
	mDynamicsWorld->addRigidBody(mVehicleBody);

	mVehicleRaycaster = new btDefaultVehicleRaycaster(mDynamicsWorld);
	mRaycastVehicle = new btRaycastVehicle(mVehicleTuning, mVehicleBody, mVehicleRaycaster);
	mRaycastVehicle->setCoordinateSystem(0, 1, 2);
	mDynamicsWorld->addVehicle(mRaycastVehicle);
	
}
void Physics::addWheel(btScalar wheelWidth, btScalar wheelRadius, btVector3 &offset, bool isFrontWheel)
{
	btVector3 wheelDirection(0.0, -1.0, 0.0);
	btVector3 wheelAxis(-1.0, 0.0, 0.0);

	btWheelInfo &wheelInfo = mRaycastVehicle->addWheel(offset, wheelDirection, wheelAxis, CAR_SUSPENSION_REST_LENGTH, wheelRadius, mVehicleTuning, isFrontWheel);
	wheelInfo.m_suspensionStiffness = CAR_SUSPENSION_STIFFNESS;
	wheelInfo.m_wheelsDampingRelaxation = CAR_SUSPENSION_DAMPING;
	wheelInfo.m_wheelsDampingCompression = CAR_SUSPENSION_COMPRESSION;
	wheelInfo.m_frictionSlip = CAR_WHEEL_FRICTION;
	wheelInfo.m_rollInfluence = CAR_ROLL_INFLUENCE;
}
void Physics::goVehicleFoward()
{
	if(mRaycastVehicle->getCurrentSpeedKmHour() < 0.0)
	{
		mRaycastVehicle->setBrake(CAR_BREAKING_FORCE, W_RearLeft);
		mRaycastVehicle->setBrake(CAR_BREAKING_FORCE, W_RearRight);

		mRaycastVehicle->applyEngineForce(0.0, W_RearLeft);
		mRaycastVehicle->applyEngineForce(0.0, W_RearRight);
	}
	else
	{
		mRaycastVehicle->applyEngineForce(CAR_ENGINE_FORCE, W_RearLeft);
		mRaycastVehicle->applyEngineForce(CAR_ENGINE_FORCE, W_RearRight);
	}
}
void Physics::goVehicleBackward()
{
	if(mRaycastVehicle->getCurrentSpeedKmHour() > 0.0)
	{
		mRaycastVehicle->setBrake(CAR_BREAKING_FORCE, W_RearLeft);
		mRaycastVehicle->setBrake(CAR_BREAKING_FORCE, W_RearRight);

		mRaycastVehicle->applyEngineForce(0.0, W_RearLeft);
		mRaycastVehicle->applyEngineForce(0.0, W_RearRight);
	}
	else
	{
		mRaycastVehicle->applyEngineForce(-CAR_ENGINE_FORCE, W_RearLeft);
		mRaycastVehicle->applyEngineForce(-CAR_ENGINE_FORCE, W_RearRight);
	}
}
void Physics::turnVehicleLeft()
{
	btScalar currentSteeringAngle = mRaycastVehicle->getSteeringValue(0);
	if(currentSteeringAngle + CAR_STEERING_ANGLE_PER_FRAME < CAR_MAX_STEERING_ANGLE)
	{
		mRaycastVehicle->setSteeringValue(currentSteeringAngle + CAR_STEERING_ANGLE_PER_FRAME, W_FrontLeft);
		mRaycastVehicle->setSteeringValue(currentSteeringAngle + CAR_STEERING_ANGLE_PER_FRAME, W_FrontRight);
	}
}
void Physics::turnVehicleRight()
{
	btScalar currentSteeringAngle = mRaycastVehicle->getSteeringValue(0);
	if(currentSteeringAngle - CAR_STEERING_ANGLE_PER_FRAME > -CAR_MAX_STEERING_ANGLE)
	{
		mRaycastVehicle->setSteeringValue(currentSteeringAngle - CAR_STEERING_ANGLE_PER_FRAME, W_FrontLeft);
		mRaycastVehicle->setSteeringValue(currentSteeringAngle - CAR_STEERING_ANGLE_PER_FRAME, W_FrontRight);
	}
}
void Physics::idleVehicle()
{
	mRaycastVehicle->applyEngineForce(0.0, W_RearLeft);
	mRaycastVehicle->applyEngineForce(0.0, W_RearRight);
}


void Physics::moveVehicle(float dx, float dy, float dz){ 
	btTransform tr;  
	// position of the object in world coords //  
	btVector3 myPos = mVehicleBody->getWorldTransform().getOrigin();
	btQuaternion myQuat = mVehicleBody->getOrientation(); // this is what you can manipulate with arrow keys // etc.  
	
	mVehicleBody->getMotionState()->getWorldTransform( tr );
	// set the position //  
	tr.setOrigin( btVector3( btScalar(myPos.x()), btScalar(myPos.y()), btScalar(myPos.z())) );   
	// zero out rotation, if you want //  
	//tr.getBasis().setEulerZYX( 0,0,0 );   
	// apply your quat rotation to the transform //  
		tr.setRotation( btQuaternion(btVector3(myQuat.x()+0.1, myQuat.y()+0.1, myQuat.z()+0.1), myQuat.w()) );   
	// apply your new transform to the rigid body //  
	//mVehicleBody->getMotionState()->setWorldTransform( tr );
	mVehicleBody->setWorldTransform(tr);
}


void Physics::applyForceToVehicle(float dx, float dy, float dz){
	mVehicleBody->applyCentralForce(btVector3(dx, dy, dz));
}

void Physics::applyTorqueToVehicle(float dx, float dy, float dz){
	
	mVehicleBody->applyTorque(btVector3(dx*10, dy*10, dz*10));

}

void Physics::setGravity(float g){
	mDynamicsWorld->setGravity(btVector3(0.0,g,0.0));
}


void Physics::setPoseOfVehicle(float azi, float pitch, float roll){
	//mVehicleBody->setAngularFactor(btVector3(azi, pitch, roll));
}